#ifndef QNODE_H
#define QNODE_H

#ifndef Q_MOC_RUN
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#endif

#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <ros/network.h>
#include <ros/ros.h>
#include <ros_qt_msgs/AdjustSpeed.h>
#include <ros_qt_msgs/Msg.h>
#include <ros_qt_msgs/NavStatus.h>
#include <ros_qt_msgs/RefReset.h>
#include <ros_qt_msgs/Relocation.h>
#include <ros_qt_msgs/SetMoveMode.h>
#include <ros_qt_msgs/TargetPose.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_srvs/SetBool.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <QBoxLayout>
#include <QDebug>
#include <QDir>
#include <QFile>
#include <QMenuBar>
#include <QSplitter>
#include <QThread>
#include <QTimer>

#include "visualization_frame.h"

namespace qnode {

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

class Publishers {
 public:
  ros::NodeHandle nh{"ros_qt"};

  ros::Publisher currentSpeedPub{nh.advertise<std_msgs::Float64MultiArray>("current_speed", 1)};
  ros::Publisher initialPosePub{nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1)};
  ros::ServiceClient forceSlowClient{nh.serviceClient<std_srvs::SetBool>("force_slow")};
  ros::ServiceClient lockLocationClient{nh.serviceClient<std_srvs::SetBool>("/can_initial_pose")};
  //  ros::ServiceClient reflectiveLocalizationEnableClient{nh.serviceClient<std_srvs::SetBool>("/reflective_localization_enable")};
  //  ros::ServiceClient reflectiveLocalizationResetClient{nh.serviceClient<ros_qt_msgs::RefReset>("/reflective_localization_reset")};
};

class QNode : public nodelet::Nodelet, public QObject {
  Q_OBJECT

 Q_SIGNALS:
  void sigSendTips(const QString &);
  void sigRosShutdown();  //正常运行时候发生严重错误，再调用发生给qt
  void sigcurrentPose(const QList<double> &);
  void sigcurrentBoardPose(const QList<double> &);
  void sigtargetSpeed(const QList<double> &);
  void sigonGoalReached();
  void sigonNavStatus(const int &);
  void sigonLocalizationLose();

 private:
  pthread_t tid;

  bool is_rosmaster_run = false;
  bool is_qnode_inited = false;
  boost::shared_ptr<MoveBaseClient> client;
  boost::shared_ptr<Publishers> publishers;
  boost::shared_ptr<tf2_ros::Buffer> tf_buffer;
  boost::shared_ptr<tf2_ros::TransformListener> tf_listener;

  ros::Timer map_timer;
  ros::Timer board_timer;

  //  void run() {
  //    int code = main();
  //    if (code < 0) { Q_EMIT sigRosShutdown(); }
  //  }

 public:
  QNode() {}

  ~QNode() { pthread_join(tid, NULL); }

  void onInit() override {
    ros::NodeHandle nh = getNodeHandle();
    pthread_create(&tid, NULL, MainLoop, this);
  }

  static void *MainLoop(void *tmp) {
    QNode *qNode = (QNode *)tmp;

    qNode->main();

    return 0;
  }

  int main() {
    if (!ros::master::check()) {
      Q_EMIT sigSendTips(QString("错误：ros master 可能未运行"));
      return -1;
    }

    ros::NodeHandle nh("ros_qt");

    is_rosmaster_run = true;

    publishers = boost::make_shared<Publishers>();

    ros::Subscriber sub = nh.subscribe<std_msgs::Float64MultiArray>("target_speed", 1, &QNode::targetSpeedSub_CB, this);
    ros::Subscriber sub5 = nh.subscribe<ros_qt_msgs::NavStatus>("/move_base/nav_status", 1, &QNode::onNavStatus, this);
    ros::Subscriber sub6 = nh.subscribe<std_msgs::Empty>("/localization_guarder", 1, &QNode::onLocalizationLose, this);

    if (!publishers->lockLocationClient.exists()) { Q_EMIT sigSendTips(QString("错误：ros-定位锁定服务未开启")); }
    if (!publishers->forceSlowClient.exists()) { Q_EMIT sigSendTips(QString("错误：ros-强制慢速服务未开启")); }

    client = boost::make_shared<MoveBaseClient>("move_base", true);

    tf_buffer = boost::make_shared<tf2_ros::Buffer>(ros::Duration(5));
    tf_buffer->setUsingDedicatedThread(true);
    tf_listener = boost::make_shared<tf2_ros::TransformListener>(*tf_buffer);

    map_timer = nh.createTimer(ros::Duration(0.1), &QNode::currentMapPoseTimer_CB, this, false, true);

    // if (!publishers->reflectiveLocalizationEnableClient.exists()) { Q_EMIT sigSendTips(QString("错误：ros-反光板定位服务未开启")); }
    // if (!publishers->reflectiveLocalizationResetClient.exists()) { Q_EMIT sigSendTips(QString("错误：ros-反光板重置服务未开启")); }
    //  board_timer = nh.createTimer(ros::Duration(0.1), &QNode::currentBoardPoseTimer_CB, this, false, false);

    Q_EMIT sigSendTips(QString("提示：ros qnode 初始化完成"));
    is_qnode_inited = true;

    ros::spin();

    return 0;
  }

  ////////////////////////////一些回调

  /**
   * @brief 导航行进状态回调（status.type<0 时无法导航，==0 时正常，>0 时还有路走但不能到终点） status.msg <- 这是对应文本消息
   * @param status
   */
  void onNavStatus(const ros_qt_msgs::NavStatus status) { Q_EMIT sigonNavStatus(static_cast<int>(status.type)); }

  /**
   * @brief 定位丢失
   */
  void onLocalizationLose(const std_msgs::Empty) { Q_EMIT sigonLocalizationLose(); }

  void move_base_CB(const actionlib::SimpleClientGoalState &state, const boost::shared_ptr<const move_base_msgs::MoveBaseResult> &) {
    if (state.isDone()) {
      switch (state.state_) {
        case actionlib::SimpleClientGoalState::SUCCEEDED: Q_EMIT sigonGoalReached(); break;
        case actionlib::SimpleClientGoalState::PREEMPTED: Q_EMIT sigSendTips(QString("SLAM导航——%1").arg(state.getText().c_str())); break;
        case actionlib::SimpleClientGoalState::PENDING:
        case actionlib::SimpleClientGoalState::ACTIVE:
        case actionlib::SimpleClientGoalState::RECALLED:
        case actionlib::SimpleClientGoalState::REJECTED:
        case actionlib::SimpleClientGoalState::ABORTED:
        case actionlib::SimpleClientGoalState::LOST: Q_EMIT sigSendTips(QString("错误：SLAM导航异常——%1").arg(state.getText().c_str())); break;
      }
    }
  }

  /**
   * @brief 当前位姿回调，订阅参数单位m
   * @param pose
   */
  void currentMapPoseTimer_CB(const ros::TimerEvent &) {
    static unsigned char tips_flag = -1;
    try {
      geometry_msgs::TransformStamped tran = tf_buffer->lookupTransform("map", "base_footprint", ros::Time(0), ros::Duration(1));
      QList<double> poselist;
      poselist.append(tran.transform.translation.x);         // x
      poselist.append(tran.transform.translation.y);         // y
      poselist.append(tf::getYaw(tran.transform.rotation));  // th
      Q_EMIT sigcurrentPose(poselist);
      if (tips_flag != 1) {
        tips_flag = 1;
        Q_EMIT sigSendTips(QString("获取SLAM定位成功[map]"));
      }
    } catch (std::runtime_error &ex) {
      if (tips_flag != 0) {
        tips_flag = 0;
        Q_EMIT sigSendTips(QString("错误：获取SLAM定位异常[map]——%1").arg(ex.what()));
      }
    }
  }

  //  /**
  //   * @brief 当前位姿回调，订阅参数单位m
  //   * @param pose
  //   */
  //  void currentBoardPoseTimer_CB(const ros::TimerEvent &) {
  //    static unsigned char tips_flag = -1;
  //    try {
  //      geometry_msgs::TransformStamped tran = tf_buffer->lookupTransform("board", "base_footprint", ros::Time(0), ros::Duration(1));
  //      QList<double> poselist;
  //      poselist.append(tran.transform.translation.x);         // x
  //      poselist.append(tran.transform.translation.y);         // y
  //      poselist.append(tf::getYaw(tran.transform.rotation));  // th
  //      Q_EMIT sigcurrentBoardPose(poselist);
  //      if (tips_flag != 1) {
  //        tips_flag = 1;
  //        Q_EMIT sigSendTips(QString("获取SLAM定位成功[board]"));
  //      }
  //    } catch (std::runtime_error &ex) {
  //      if (tips_flag != 0) {
  //        tips_flag = 0;
  //        Q_EMIT sigSendTips(QString("错误：获取SLAM定位异常[board]——%1").arg(ex.what()));
  //      }
  //    }
  //  }

  /**
   * @brief 订阅导航速度，单位m/s
   * @param motor
   */
  void targetSpeedSub_CB(const std_msgs::Float64MultiArray motor) {
    QList<double> speed;
    speed.append(motor.data[0]);  // left
    speed.append(motor.data[1]);  // right
    Q_EMIT sigtargetSpeed(speed);
    // Q_EMIT sigSendTips( QString( "l:%1 r:%2" ).arg( motor.data[ 0 ] ).arg(
    // motor.data[ 1 ] ) );
  }

  ////////////////////////////可调用

  /**
   * @brief 传入参数单位 m
   * @param start
   * @param target
   * @param end_yaw
   * @param mode_value
   * @param motion_dir
   */
  void setGoal(const std::pair<double, double> &start, const std::vector<std::pair<double, double>> &targets, const double &target_yaw, const std::vector<std::pair<double, double>> &speed_limits, const int mode_value, const int motion_dir) {
    if (!is_qnode_inited) { return; }

    if (targets.empty()) {
      Q_EMIT sigSendTips(QString("错误：qnode: 无目标点！"));
      return;
    }

    std_msgs::Header header;
    header.frame_id = mode_value != 3 ? "map" : "board";

    move_base_msgs::MoveBaseGoal goal;
    goal.global_.startWithSpecificPose = true;
    goal.global_.start_pose.header = header;
    goal.global_.start_pose.pose.position.x = start.first;
    goal.global_.start_pose.pose.position.y = start.second;
    goal.global_.start_pose.pose.orientation.w = 1;

    for (auto &target : targets) {
      geometry_msgs::PoseStamped g;
      g.header = header;
      g.pose.position.x = target.first;
      g.pose.position.y = target.second;
      g.pose.orientation.w = 1;
      goal.global_.target_poses.push_back(g);
    }

    goal.global_.target_poses.back().pose.orientation = tf::createQuaternionMsgFromYaw(target_yaw);
    goal.global_.is_auto_avoid = mode_value > 1 ? false : true;
    goal.local_.is_dock = false;
    goal.local_.motion_dir = motion_dir == -1 ? -1 : 1;

    for (auto &speed_limit : speed_limits) {
      ros_qt_msgs::DistAndSpeed tmp;
      tmp.dist = speed_limit.first;
      tmp.speed = speed_limit.second;
      goal.local_.speed_limit.push_back(tmp);
    }

    try {
      if (!client->waitForServer(ros::Duration(3))) {
        Q_EMIT sigSendTips(QString("错误：无法连接SLAM导航服务！"));
        return;
      }

      client->sendGoal(goal, boost::bind(&QNode::move_base_CB, this, _1, _2));
    } catch (std::runtime_error &err) { Q_EMIT sigSendTips(QString("错误：无法连接SLAM导航服务——%1").arg(err.what())); }
  }

  /**
   * @brief 取消导航
   */
  void cancel() {
    if (!is_qnode_inited) { return; }

    client->cancelAllGoals();
  }

  //  /**
  //   * @brief 使能反光板定位，返回是否开启成功
  //   */
  //  bool enableReflectiveLocalization(bool enable) {
  //    if (!is_qnode_inited) { return false; }

  //    if (enable) {
  //      board_timer.start();
  //    } else {
  //      board_timer.stop();
  //    }

  //    if (publishers->reflectiveLocalizationEnableClient.exists()) {
  //      std_srvs::SetBool data;
  //      data.request.data = enable;
  //      return publishers->reflectiveLocalizationEnableClient.call(data) && data.response.success;
  //    } else {
  //      Q_EMIT sigSendTips(QString("错误：ros reflectiveLocalizationEnableClient服务不存在！"));
  //      return false;
  //    }
  //  }

  //  /**
  //   * @brief 重置反光板参考全局坐标，且使能反光板定位
  //   */
  //  bool reflectiveReset(double x, double y) {
  //    if (!is_qnode_inited) { return false; }

  //    if (publishers->reflectiveLocalizationResetClient.exists()) {
  //      ros_qt_msgs::RefReset data;
  //      data.request.use_global_position = true;
  //      data.request.x = x;
  //      data.request.y = y;
  //      return publishers->reflectiveLocalizationResetClient.call(data) && data.response.success;
  //    } else {
  //      Q_EMIT sigSendTips(QString("错误：ros reflectiveLocalizationResetClient服务不存在！"));
  //      return false;
  //    }
  //  }

  /**
   * @brief 发布轮子实际速度，单位m/s
   * @param left
   * @param right
   */
  void uploadSpeed(const double &left, const double &right) {
    if (!is_qnode_inited) { return; }
    std_msgs::Float64MultiArray msg;
    msg.data.resize(2);
    msg.data[0] = left;
    msg.data[1] = right;
    publishers->currentSpeedPub.publish(msg);
    // Q_EMIT sigSendTips(QString("l:%1 r:%2").arg(msg.data[0]).arg(msg.data[1]));
  }

  /**
   * @brief 强制慢速
   * @param slow
   * @return
   */
  bool setForceSlow(bool slow) {
    if (!is_qnode_inited) { return false; }
    if (publishers->forceSlowClient.exists()) {
      std_srvs::SetBool data;
      data.request.data = slow;
      return publishers->forceSlowClient.call(data) && data.response.success;
    } else {
      Q_EMIT sigSendTips(QString("错误：ros setForceSlow服务不存在！"));
      return false;
    }
  }

  /**
   * @brief 锁定AMCL重定位
   * @param lock
   * @return
   */
  bool lockLocation(bool lock) {
    if (!is_qnode_inited) { return false; }
    if (publishers->lockLocationClient.exists()) {
      std_srvs::SetBool data;
      data.request.data = !lock;
      return publishers->lockLocationClient.call(data) && data.response.success;
    } else {
      Q_EMIT sigSendTips(QString("错误：ros lockLocation服务不存在！"));
      return false;
    }
  }

  /**
   * @brief AMCL重定位
   * @param x
   * @param y
   * @param yaw
   * @return
   */
  bool relocation(double x, double y, double yaw) {
    if (!is_qnode_inited) { return false; }

    geometry_msgs::PoseWithCovarianceStamped msg;
    msg.header.frame_id = "map";
    msg.header.stamp = ros::Time::now();
    msg.pose.pose.position.x = x;
    msg.pose.pose.position.y = y;
    msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
    msg.pose.covariance = {0.25, 0.0,  0.0, 0.0, 0.0, 0.0,  //
                           0.0,  0.25, 0.0, 0.0, 0.0, 0.0,  //
                           0.0,  0.0,  0.0, 0.0, 0.0, 0.0,  //
                           0.0,  0.0,  0.0, 0.0, 0.0, 0.0,  //
                           0.0,  0.0,  0.0, 0.0, 0.0, 0.0,  //
                           0.0,  0.0,  0.0, 0.0, 0.0, 0.06853891945200942};
    publishers->initialPosePub.publish(msg);
    return true;
  }
};

}  // namespace qnode

#endif  // QNODE_H
